#include "vicsek.h"
#include<string>
using namespace std;

Vicsek::Vicsek(int n, int k, float l, int m)
{
	N = n;
	K = k;
	L = l;
	M = m;
}

void Vicsek::Init()
{
	x = new float [N];
	y = new float [N];
	vx = new float [N];
	vy = new float [N];

	memset(x, 0, sizeof(float)*N);
	memset(y, 0, sizeof(float)*N);
	memset(vx, 0, sizeof(float)*N);
	memset(vy, 0, sizeof(float)*N);

	c = new int [N];
	memset(c, -1, sizeof(int)*N);
	cs = new int [M*M];
	memset(cs, 0, sizeof(int)*M*M);
	nlist = new int [K*N];
	memset(nlist, -1, sizeof(int)*N*K);
	AF = new float [N*2];
	memset(AF, 0, sizeof(float)*N*2);
	CF = new float [N*2];
	memset(CF, 0, sizeof(float)*N*2);
	NF = new float [N*2];
	memset(NF, 0, sizeof(float)*N*2);
//	pasize = new int [N];
//	memset(pasize, 0, sizeof(int)*N);
//
//	pclist = new int* [N];
//	for (int i = 0; i < N; i++)
//		pclist[i] = new int [1];
//	clist = new int* [M*M];
//	for (int i = 0; i < M*M; i++)
//		clist[i] = new int [1];
//	palist = new int* [N];
//	for (int i = 0; i < N; i++)
//		palist[i] = new int [1];
	srand(time(0));
	for (int i = 0; i < N; i++)
	{
		x[i] = ((float)rand() / RAND_MAX - 0.5)*L*0.1;
		y[i] = ((float)rand() / RAND_MAX - 0.5)*L*0.1;
		vx[i] = (float)rand() / RAND_MAX -0.5;
		vy[i] = (float)rand() / RAND_MAX -0.5;
	}
}

void Vicsek::ClearMemory()
{
    delete [] x;
    delete [] y;
    delete [] vx;
    delete [] vy;
    delete [] c;
    delete [] cs;
    delete [] AF;
    delete [] CF;
    delete [] NF;
    delete [] nlist;
}
void Vicsek::Hash_mapping()
{
	memset(cs,0,sizeof(int)*M*M);
	float gs = (2*L)/(float)M;
	int ix, iy;
	for (int i = 0; i < N; i++)
	{
		ix = (int)(x[i] + L) / gs;
		iy = (int)(y[i] + L) / gs;
		cs[ix + iy*M]++;
		c[i] = ix + iy*M;
	}
	for (int i = 0; i < M*M; i++)
		delete [] clist[i];
	delete [] clist;

	clist = new int* [M*M];
	for (int i = 0; i <M*M; i++)
		clist[i] = new int [cs[i]];
	int* cnt = new int [M*M];
	memset(cnt, 0, sizeof(int)*M*M);
	for (int i = 0; i <N; i++)
	{
		clist[c[i]][cnt[c[i]]] = i;
		cnt[c[i]]++;
	}
	delete [] cnt;
}

void Vicsek::Get_Potential_Cell_List()
{
	for (int i = 0; i < N; i++)
		delete [] pclist[i];
	delete [] pclist;
	pclist = new int* [N];
	for (int i = 0; i < N; i++)
	{
		int cx = c[i] % M;
		int cy = c[i] / M;
		pclist[i] = new int [25];
		memset(pclist[i], -1 ,sizeof(int)*25);
		int level = 0;
		int cnt = cs[c[i]];
		int tmp = 0;
		pclist[i][0] = c[i];
		while (cnt < K && level <=1)
		{
			level++;
			int ix, iy;
			//top
			iy = cy - level;
			for ( ix = cx - level; ix < cx + level; ix++) {
				tmp++;
				if (ix >=0 && ix < M && iy>=0 && iy<M) {
					pclist[i][tmp] = ix + iy*M;
					cnt += cs[ix + iy*M];
				} else {
					pclist[i][tmp] = -1;
				}
			}
			//right
			ix = cx + level;
			for ( iy = cy - level; iy < cy + level; iy++) {
				tmp++;
				if (ix >=0 && ix < M && iy>=0 && iy<M) {
					pclist[i][tmp] = ix + iy*M;
					cnt += cs[ix + iy*M];
				} else {
					pclist[i][tmp] = -1;
				}
			}
			//bottom
			iy = cy + level;
			for (ix = cx - level + 1; ix < cx + level;ix++) {
				tmp++;
				if (ix >=0 && ix < M && iy>=0 && iy<M) {
					pclist[i][tmp] = ix + iy*M;
					cnt += cs[ix + iy*M];
				} else {
					pclist[i][tmp] = -1;
				}
			}
			//left
			ix = cx - level;
			for( iy = cy - level + 1; iy < cy + level; iy++) {
				tmp++;
				if (ix >=0 && ix < M && iy>=0 && iy<M) {
					pclist[i][tmp] = ix + iy*M;
					cnt += cs[ix + iy*M];
				} else {
					pclist[i][tmp] = -1;
				}
			}

		}
	}
}

void Vicsek::Get_Potential_Agent_List()
{
	for (int i = 0; i < N; i++)
		delete [] palist[i];
	delete [] palist;
	palist = new int* [N];

	memset(pasize,0,sizeof(int)*N);
	for (int i = 0; i < N; i++){
		int cnt = 0;
		for (int j = 0; j < 25; j++){
			if (pclist[i][j] != -1)
				cnt += cs[pclist[i][j]];
		}
		palist[i] = new int [cnt];
		pasize[i] = cnt;
		int pos = 0;
		for (int j = 0; j < 25; j++){
			if (pclist[i][j] != -1){
				for (int k = 0; k < cs[pclist[i][j]]; k++)
					palist[i][pos+k] = clist[pclist[i][j]][k];
				pos += cs[pclist[i][j]];
			}
		}
	}
}


void Vicsek::Find_Kth_Min(float* data, int n,int K,int* id)
{
	for (int i = 0; i < n; i++)
		id[i] = i;
	if ( K >=n ) return ;
	bool flag = false;
	int k = K;
	int low = 0;
	int high = n - 1;
	while ( !flag )
	{
		int key = id[low];
		int x = low;
		int y = high;
		while ( x < y )
		{
			while( data[key] <= data[id[y]] && x < y) y--;
			id[x] = id[y];
			while( data[id[x]] <= data[key] && x < y) x++;
			id[y] = id[x];
		}
		id[x] = key;
		int cnt = x - low + 1;
		if (k == cnt ) flag = true;
		if ( k > cnt ) { k = k - cnt; low = x + 1;}
		else { high = x - 1; }
	}
}

void Vicsek::Search_K_Neighbors()
{
	//for (int i = 0; i < N; i++){
	//	float* dst = new float [pasize[i]];
	//	int* index = new int [pasize[i]];
	//	for (int j = 0; j < pasize[i]; j++){
	//		float a = x[i]-x[palist[i][j]];
	//		float b = y[i]-y[palist[i][j]];
	//		dst[j] = a*a+b*b;
	//	}
	//	Find_Kth_Min(dst, pasize[i], K, index);
	//	for (int j = 0; j < K; j++)
	//		nlist[i*K+j] = palist[i][index[j]];
	//	delete [] dst;
	//	delete [] index;
	//}
    #pragma omp parallel for
	for (int i = 0; i < N; i++)
	{
		float* dst = new float [N];
		int* index = new int [N];
		for (int j = 0; j < N; j++)
				dst[j] = (x[i]-x[j])*(x[i]-x[j]) + (y[i]-y[j])*(y[i]-y[j]);
		Find_Kth_Min(dst, N, K, index);
		for (int j = 0; j < K; j++)
			nlist[i*K+j] = index[j];
		delete [] index;
		delete [] dst;
	}
}

void Vicsek::Get_Alignment_Force()
{
	#pragma  omp parallel for
	for (int i = 0; i < N; i++){
		AF[i*2]=0;
		AF[i*2+1]=0;
		for (int j = 0; j < K; j++){
			AF[i*2] += vx[nlist[i*K+j]];
			AF[i*2+1] += vy[nlist[i*K+j]];
		}
	}
}

void Vicsek::Get_Cohesion_Force()
{
	#pragma  omp parallel for
	for (int i = 0; i < N; i++){
		CF[i*2] = 0;
		CF[i*2+1] = 0;
		for (int j = 0; j < K; j++){
			if (i != nlist[i*K+j]){
				float ax = x[nlist[i*K+j]] - x[i];
				float ay = y[nlist[i*K+j]] - y[i];
				float r = sqrt(ax*ax+ay*ay);
				float r0 = 0.03;
				float re = 2;
				float ex = ax/r;
				float ey = ay/r;
				if ( r < r0){
					CF[i*2] +=  ex * (-10); CF[i*2+1] += ey * (-10);
				}else if (r < 2*re)
				{
					CF[i*2] += ex * (r - re)/ re;
					CF[i*2+1] += ey*(re - re)/re;
				}else {
					CF[i*2] += ex;
					CF[i*2+1] += ey;
				}
			}
		}
	}
}


void Vicsek::Get_Noise_Force()
{
	#pragma  omp parallel for
	for (int i = 0; i < N; i++){
		float theta = (2*(float)rand()/RAND_MAX - 1)*2*3.1415926;
		NF[2*i] = cos(theta);
		NF[2*i+1] = sin(theta);
	}
}

void Vicsek::Update_Speed(float a, float b, float c)
{
	#pragma  omp parallel for
	for (int i = 0; i < N; i++){
		vx[i] += (a*AF[2*i] + b*CF[2*i] + c*NF[2*i]);
		vy[i] += (a*AF[2*i+1] + b*CF[2*i+1] + c*NF[2*i+1]);
		float r = sqrt(vx[i]*vx[i] + vy[i]*vy[i]);
		vx[i] = vx[i] / r;
		vy[i] = vy[i] / r;
	}
}

void Vicsek::Update_Position()
{

	float timestep = 0.05;
	#pragma  omp parallel for
	for (int i = 0; i < N; i++){
		x[i] += vx[i] * timestep;
		y[i] += vy[i] * timestep;
		if ( x[i] < -L ) x[i] += 2*L;
		if ( x[i] > L ) x[i] -= 2*L;
		if ( y[i] < -L ) y[i] += 2*L;
		if ( y[i] > L ) y[i] -= 2*L;
	}
}

void Vicsek::Add_Intrinsic_Noise(float noise)
{
	for (int i = 0; i < N; i++)
	{
		float theta = ((float)rand()/RAND_MAX - 0.5)*2*noise*2*3.1415926;
		float a = x[i];
		float b = y[i];
		x[i] = a*cos(theta) - b * sin(theta);
		y[i] = a*sin(theta) + b * cos(theta);
	}
}
//
//void Vicsek::Write_File()
//{
//	string prefix = "PV";
//	time_t t = time(0);
//	char tmp[64];
//	strftime( tmp, sizeof(tmp), "%Y_%m_%d_%X",localtime(&t) );
//	string filename(tmp);
//	filename = prefix + string("_") + filename;
//	filename = filename + string(".txt");
//	string route("../data/");
//	filename = route + filename;
//	int location = 0;
//	while (1)
//	{
//		location = filename.find(":");
//		if(location == -1 )
//			break;
//		else
//			filename[location] = '_';
//	}
//	ofstream out(filename);
//	for (int i = 0; i < N; i++){
//		out<<x[i]<<" "<<y[i]<<" "<<vx[i]<<" "<<vy[i]<<endl;
//	}
//	out.close();
//}

void Vicsek::CreateFile(int ndir, int nsubfolders, int nfile)
{
    char buff[10];
    sprintf(buff, "%d", ndir);
    string dirname = buff;
    char buff1[10];
    sprintf(buff1, "%d", nsubfolders);
    string dirname1= buff1;
    char buff2[10];
    sprintf(buff2, "%d", nfile);
    string filename = buff2;
    filename = "./"+dirname +"/"+dirname1+"/"+filename+".txt";
    ofstream outfile(filename.c_str());
    for (int i = 0; i < this->N; i++)
        outfile<<x[i]<<" "<<y[i]<<" "<<vx[i]<<" "<<vy[i]<<endl;
    outfile.close();
}

void Vicsek::CreateParFile(int ndir, float Alpha, float Beta, float Gamma)
{
    char buff[10];
    sprintf(buff, "%d", ndir);
    string dirname = buff;
    string filename = "./"+dirname+"/"+"0.txt";
    ofstream outfile(filename.c_str());
    outfile<<"N "<<this->N<<endl;
    outfile<<"K "<<this->K<<endl;
    outfile<<"alpha "<<Alpha<<endl;
    outfile<<"beta "<<Beta<<endl;
    outfile<<"Gamma "<<Gamma<<endl;
    outfile<<"L "<<this->L<<endl;
    outfile<<"M "<<this->M<<endl;
    outfile.close();
}
void Vicsek::Simulate(float Alpha, float Beta, float Gamma)
{
	static int tmp = 0;
	//Hash_mapping();
	//Get_Potential_Cell_List();
	//Get_Potential_Agent_List();
	Search_K_Neighbors();
	Get_Alignment_Force();
	Get_Cohesion_Force();
	Get_Noise_Force();
	Update_Speed(Alpha, Beta, Gamma);
	//Add_Intrinsic_Noise( 0.04 );
	Update_Position();
	tmp++;
	//test_order();
	//Write_File();
}
